#include "concatenate_clouds.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
using namespace pcl;

pcl::PointCloud<pcl::PointXYZ> ConcatCloudFields(pcl::PointCloud<pcl::PointXYZ> cloud_a, pcl::PointCloud<pcl::PointXYZ> cloud_b)
{
	
	pcl::PointCloud<pcl::PointXYZ> cloud_c;
	pcl::concatenateFields(cloud_a, cloud_b, cloud_c);
	return cloud_c;
}

pcl::PointCloud<pcl::PointXYZ> ConcatC(pcl::PointCloud<pcl::PointXYZ> cloud_a, pcl::PointCloud<pcl::PointXYZ> cloud_b)
{
	pcl::PointCloud<pcl::PointXYZ> cloud_c;
	cloud_c = cloud_a;
	cloud_c += cloud_b;
	return cloud_c;
}

int concatenate_clouds::ConcatFields(std::string patha, std::string pathb, std::string pathResult)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_a(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>(patha, *cloud_a);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_b(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>(pathb, *cloud_b);

	pcl::PointCloud<pcl::PointXYZ> cloud_c = ConcatCloudFields(*cloud_a, *cloud_b);
	pcl::io::savePCDFileASCII(pathResult, cloud_c);
	return 0;
}

int concatenate_clouds::Concat(std::string patha, std::string pathb, std::string pathResult)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_a(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>(patha, *cloud_a);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_b(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>(pathb, *cloud_b);

	pcl::PointCloud<pcl::PointXYZ> cloud_c = ConcatC(*cloud_a, *cloud_b);
	pcl::io::savePCDFileASCII(pathResult, cloud_c);
	return 0;
}
